#include <rclcpp/rclcpp.hpp>
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
#include <chrono>
#include <thread>

class InitializePose : public rclcpp::Node
{
public:
    InitializePose() : Node("initialize_pose")
    {
        // 创建一个publisher，发布到 /initialpose 话题
        initial_pose_pub_ = this->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>(
            "/initialpose", 5);
        
        // 创建一个定时器，每隔1秒调用一次
        timer_ = this->create_wall_timer(
            std::chrono::seconds(1),
            std::bind(&InitializePose::set_initial_pose, this));
        start_time_ = this->now();
    }

private:
    void set_initial_pose()
    {
        auto current_time = this->now();
        auto elapsed_time = current_time - start_time_;
        // 如果超过时间，则停止定时器
        if (elapsed_time.seconds() >= 15)
        {
            RCLCPP_INFO(this->get_logger(), "已超时，停止发送初始位姿");
            timer_->cancel();
            return;
        }
        // 检查AMCL节点是否启动
        if (!amcl_started_)
        {
            auto node_names = this->get_node_names();
            for (const auto &name : node_names)
            {
                RCLCPP_INFO(this->get_logger(), "node = %s", name.c_str());
                if(name == "/amcl"){
                    amcl_started_ = true;
                    break;
                }
            }
        }
        if(!amcl_started_){
            RCLCPP_WARN(this->get_logger(), "AMCL节点未启动");
            return;
        }
        // 创建一个PoseWithCovarianceStamped消息
        auto initial_pose = geometry_msgs::msg::PoseWithCovarianceStamped();
        initial_pose.header.frame_id = "map";
        initial_pose.header.stamp = this->get_clock()->now();

        // 设置初始位置和方向
        initial_pose.pose.pose.position.x = 0; // 初始x坐标
        initial_pose.pose.pose.position.y = 0; // 初始y坐标
        initial_pose.pose.pose.position.z = 0.0;

        initial_pose.pose.pose.orientation.x = 0;
        initial_pose.pose.pose.orientation.y = 0;
        initial_pose.pose.pose.orientation.z = 0;
        initial_pose.pose.pose.orientation.w = 1.0; // 初始方向（四元数）

        // 设置协方差矩阵（可选）
        initial_pose.pose.covariance[6 * 0 + 0] = 0.25;                // x位置的方差
        initial_pose.pose.covariance[6 * 1 + 1] = 0.25;                // y位置的方差
        initial_pose.pose.covariance[6 * 5 + 5] = 0.06853891945200942; // yaw 轴方差

        // 发布初始姿态
        initial_pose_pub_->publish(initial_pose);
        RCLCPP_INFO(this->get_logger(), "initialize_pose 发送初始位姿");
        // 等待一段时间，确保消息被处理
        std::this_thread::sleep_for(std::chrono::seconds(1));
    }

    rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr initial_pose_pub_;
    rclcpp::TimerBase::SharedPtr timer_;
    rclcpp::Time start_time_;
    bool amcl_started_ = false;
};

int main(int argc, char *argv[])
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<InitializePose>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}